#include "ccalculation.h"
#include <math.h>
#include <stdio.h>
using namespace std;

//================================================
CCalculation::CCalculation(GeometryData *data)
{
 this->data = data;
}

//================================================
void CCalculation::buildControlValues()
{
    //calculate CV areas
    for (int i =0; i<data->triangles.size(); i++)
    {
        Triangle * t = data->triangles[i];

        t->p1->cv.s += t->area() / 3.0;
        t->p2->cv.s += t->area() / 3.0;
        t->p3->cv.s += t->area() / 3.0;
    }
    //find max, min, averege
    ControlValue::min = data->points[0]->cv.s;
    ControlValue::max = 0;
    double cv_sum = 0;

    for (int i =0; i<data->points.size(); i++)
    {
        Point *p = data->points[i];
        ControlValue::min = min(ControlValue::min, p->cv.s);
        ControlValue::max = max(ControlValue::max, p->cv.s);
        cv_sum += p->cv.s;
    }

    ControlValue::avg = cv_sum / data->points.size();
}

//================================================
void CCalculation::initTimeLoop()
{
    tau = 1/(ControlValue::max*ControlValue::max);
    max_time = 100000;
    eps = 0.0001;
}

//================================================
void solve_jacobi (int N, double **A, double *F, double *X, double eps)
{
    double * TempX = new double[N];
    double norm; // норма, определяемая как наибольшая разность компонент столбца иксов соседних итераций.

    do {
        for (int i = 0; i < N; i++) {
            TempX[i] = F[i];
            for (int g = 0; g < N; g++) {
                if (i != g)
                    TempX[i] -= A[i][g] * X[g];
            }
            TempX[i] /= A[i][i];
        }
                norm = fabs(X[0] - TempX[0]);
        for (int h = 0; h < N; h++) {
            if (fabs(X[h] - TempX[h]) > norm)
                norm = fabs(X[h] - TempX[h]);
            X[h] = TempX[h];
        }
    } while (norm > eps);
    delete[] TempX;
}

//================================================
void CCalculation::setDefaults()
{
    for (int i = 0; i < data->points.size(); i++)
    {
        data->points[i]->psi = 0;
        data->points[i]->omega = 0;
        data->points[i]->temp = 0;
    }

    for (int i =0; i < data->edges.size(); i++)
    {
        if(data->edges[i]->type == Edge::IN || data->edges[i]->type == Edge::OUT)
        {
            data->edges[i]->p1->psi = 0;
            data->edges[i]->p2->psi = 0;

            data->edges[i]->p1->omega = 0;
            data->edges[i]->p2->omega = 0;
        }
    }
}

//================================================
void CCalculation::doCalculation()
{

    //calculate variables for cv: area
    //maximum
    //minimum
    //averege

    setDefaults();

    printf("Initializing control values...");
    buildControlValues();

    //calculate tau, max, min, epsilon
    initTimeLoop();

    //do time loop
    for (double time = 0; time < max_time; time += tau)
    {
        calculateVelocities();
        solveOmegaEquation();
        solvePsiEquation();
     //   solveTempEquation();
    }

}

//================================================
void CCalculation::calculateVelocities()
{
    for (int i =0; i < data->edges.size(); i++)
    {
        Edge *e = data->edges[i];

        if(e->type == Edge::IN)
        {
            e->p1->v.x = 0;//10*e->normal_in.x;
            e->p1->v.y = 1;//*e->normal_in.y;
            e->p2->v.x = 0;//10*e->normal_in.x;
            e->p2->v.y = 1;//*e->normal_in.y;
        }
        if(e->type == Edge::WALL)
        {
            e->p1->v.x = 0;
            e->p1->v.y = 0;
            e->p2->v.x = 0;
            e->p2->v.y = 0;
        }
        if(e->type == Edge::NONE)
        {
            e->p1->v.x = e->p1->psi / e->p1->cv.s;
            e->p1->v.y = - e->p1->psi / e->p1->cv.s;;
            e->p2->v.x = e->p2->psi / e->p2->cv.s;
            e->p2->v.y = - e->p2->psi / e->p2->cv.s;;
        }

    }
}

//================================================
void CCalculation::solvePsiEquation()
{

    //matrix must be
    //A0 * PSI = (omega*v/v_av + A+_psi+_ A-_psi-_)

    //for each CV (point)
    for (int i =0; i < data->points.size(); i++)
    {
        Point *p = data->points[i];
        int tr_count = p->triangles.size();

        std::vector <double> A0(tr_count, 0);
        std::vector <double> R(tr_count, 0);


        for (int j =0; j < tr_count; j++)
        {
            Triangle *t = p->triangles[j];

            Point *p_p = t->next_point(p);
            Point *p_m = t->prev_point(p);

            Edge *ep = data->findEdge(p,p_p);
            Edge *em = data->findEdge(p,p_m);

            double x = p->x;
            double y = p->y;

            double x_m = p_m->x;
            double x_p = p_p->x;
            double y_m = p_m->y;
            double y_p = p_p->y;

            double s = 1 / ( ControlValue::avg * 4 * t->area());

            A0[j] = s * ((x_m-x_p)*(x_m-x_p) + (y_m-y_p)*(y_m-y_p));

            double A_M = s * ((x - x_p)*(x_p - x_m) + (y - y_p)*(y_p - y_m));
            double A_P = s * ((x_m - x)*(x_p - x_m) + (y_m - y)*(y_p - y_m));

            double G_P = 0;
            double G_M = 0;

            if ( ep->isBorder() )
            {
                G_P = - 1.0 / ( ControlValue::avg * 2);
                G_P *= (ep->length()/2.0)*(ep->p1->omega + ep->p2->omega);
            }

            if ( em->isBorder() )
            {
                G_M = - 1.0 / ( ControlValue::avg * 2);
                G_M *= (em->length()/2.0)*(em->p1->omega + em->p2->omega);
            }

            R[j] = (p->cv.s/ControlValue::avg)*p->omega - A_M*em->avg_omega() - A_P*em->avg_omega() - G_M - G_P;

        }
        double new_psi = 0;
        double **A = new double *[1];
        A[0] = A0.data();

        solve_jacobi(1, A, R.data(), &new_psi,0.001);
        p->new_psi = new_psi;
    }

    for (int i = 0; i < data->points.size(); i++)
    {
        data->points[i]->psi = data->points[i]->new_psi;
    }
}

void CCalculation::solveOmegaEquation()
{
    double mu = 10;
    for (int i =0; i < data->points.size(); i++)
    {
        Point *p = data->points[i];
        int tr_count = p->triangles.size();

        std::vector <double> A0(tr_count, 0);
        std::vector <double> R(tr_count, 0);


        for (int j =0; j < tr_count; j++)
        {
            Triangle *t = p->triangles[j];

            Point *p_p = t->next_point(p);
            Point *p_m = t->prev_point(p);

            Edge *ep = data->findEdge(p,p_p);
            Edge *em = data->findEdge(p,p_m);

            double x = p->x;
            double y = p->y;

            double x_m = p_m->x;
            double x_p = p_p->x;
            double y_m = p_m->y;
            double y_p = p_p->y;

            double v_x = p->v.x;
            double v_y = p->v.y;

            double v_x_p = p_p->v.x;
            double v_x_m = p_m->v.x;
            double v_y_p = p_p->v.y;
            double v_y_m = p_m->v.y;

            double dx_p = ep->p1->x - ep->p2->x;
            double dx_m = em->p1->x - em->p2->x;

            double dy_p = ep->p1->y - ep->p2->y;
            double dy_m = em->p1->y - em->p2->y;


            double Ex_p = exp((-1 / (2 * mu))*(v_x+v_x_p)*dx_p);
            double Ex_m = exp((-1 / (2 * mu))*(v_x+v_x_m)*dx_m);
            double Ey_p = exp((-1 / (2 * mu))*(v_y+v_y_p)*dy_p);
            double Ey_m = exp((-1 / (2 * mu))*(v_y+v_y_m)*dy_m);

            double s = (3 * mu) / ( ControlValue::avg * 4 * t->area() * (Ex_m + Ex_p + 1));

            double A_0 = s * ((x_p-x_m)*(x_p-x_m) + (y_p-y_m)*(y_p-y_m));

            double A_M = -s * (Ex_m*(y_p - y)*(y_p - y_m) + Ey_m*(x_p - x)*(x_p - x_m));
            double A_P = s * (Ex_p*(y_m - y)*(y_p - y_m) + Ey_p*(x_m - x)*(x_p - x_m));

            double G_P = 0;
            double G_M = 0;

            if ( ep->isBorder() )
            {
                G_P = 1.0 / ( ControlValue::avg * 2);
                G_P *= (ep->length()/2.0)*(ep->p1->omega + ep->p2->omega);
            }

            if ( em->isBorder() )
            {
                G_M = - 1.0 / ( ControlValue::avg * 2);
                G_M *= (em->length()/2.0)*(em->p1->omega + em->p2->omega);
            }
            A0[j]  = (p->cv.s/ControlValue::avg);
            R[j] = A0[j]*p->omega + tau*(A_0*p->omega+ A_M*p_m->omega + A_P*p_p->omega+G_P+G_M);

        }
        double new_omega = 0;
        double **A = new double *[1];
        A[0] = A0.data();

        solve_jacobi(1, A, R.data(), &new_omega,0.001);
        p->new_omega = new_omega;
    }

    for (int i = 0; i < data->points.size(); i++)
    {
        data->points[i]->psi = data->points[i]->new_psi;
    }
}
